#include "MyFit.h"

MyFit::MyFit(void) : n(0)
{
}

MyFit::~MyFit()
{
}

void MyFit::setData(double t[], uint16_t raw[], int size)
{

    ///int *x=new int[size]//可以试试new一个

    if (size > 10)
        size = 10; //最多10个数据（未解决数组指针越界问题，先这样用）

    memcpy(this->x, t, size * sizeof(double));

    for (size_t i = 0; i < size; i++)
    {
        this->y[i] = (double)raw[i];
    }
    this->n = size;
}

double *MyFit::fit()
{

    if (0 == this->n)
        return 0;

    static double b[FIT_RANK + 1];

    memset(b, 0, sizeof(double) * (FIT_RANK + 1));         //每次计算都要清理，会影响下次计算的
    memset(atemp, 0, sizeof(double) * (2 * FIT_RANK + 1));

    int i, j, k;

    // for (i = 0; i < 5; i++)
    // {
    //     Serial.printf("y[%d]=%f\n", i, this->y[i]);
    // }

    for (i = 0; i < this->n; i++)
    {
        for (k = 0; k < FIT_RANK + 1; k++)
        {
            b[k] += pow(x[i], k) * (this->y[i]);

            // Serial.printf("i=%d,x=%f,y=%f,b[%d]=%f\n", i, x[i], this->y[i], k, b[k]);
        }

        for (j = 0; j < (2 * FIT_RANK + 1); j++)
        {
            atemp[j] += pow(x[i], j);
        }
    }

    atemp[0] = this->n;

    /////////////////////////////////////////////////////////////////////////////////
    // for (i = 0; i < (2 * FIT_RANK + 1); i++)
    //     printf("atemp[%d] = %f\n", i, atemp[i]);
    // printf("\n");
    // for (i = 0; i <= FIT_RANK; i++)
    //     printf("b[%d] = %f\n", i, b[i]);
    // printf("\n");
    //////////////////////////////////////////////////

    for (i = 0; i < FIT_RANK + 1; i++)
    { //构建线性方程组系数矩阵，b[]不变
        k = i;
        for (j = 0; j < FIT_RANK + 1; j++)
            a[i][j] = atemp[k++];
    }
    //////////////////////////////////////////////////////////////
    // for (i = 0; i < FIT_RANK + 1; i++)
    // {
    //     for (j = 0; j < FIT_RANK + 1; j++)
    //         printf("a[%d][%d] = %-17f  ", i, j, a[i][j]);
    //     printf("\n");
    // }
    // printf("\n");
    //////////////////////////////////////////////////////////

    //以下为高斯列主元消去法解线性方程组
    for (k = 0; k < FIT_RANK + 1 - 1; k++)
    { //n - 1列
        int column = k;
        double mainelement = a[k][k];

        for (i = k; i < FIT_RANK + 1; i++) //找主元素
            if (fabs(a[i][k]) > mainelement)
            {
                mainelement = fabs(a[i][k]);
                column = i;
            }
        for (j = k; j < FIT_RANK + 1; j++)
        { //交换两行
            double temp = a[k][j];
            a[k][j] = a[column][j];
            a[column][j] = temp;
        }
        double btemp = b[k];
        b[k] = b[column];
        b[column] = btemp;

        for (i = k + 1; i < FIT_RANK + 1; i++)
        { //消元过程
            double Mik = a[i][k] / a[k][k];
            for (j = k; j < FIT_RANK + 1; j++)
                a[i][j] -= Mik * a[k][j];
            b[i] -= Mik * b[k];
        }
    }

    ///////////////////////////////////////////////
    // for (i = 0; i < FIT_RANK + 1; i++)
    // { //经列主元高斯消去法得到的上三角阵(最后一列为常系数)
    //     for (j = 0; j < FIT_RANK + 1; j++)
    //         printf("%20f", a[i][j]);
    //     printf("%20f\n", b[i]);
    // }
    // printf("\n");
    ////////////////////////////////////////////////////////////////////

    b[FIT_RANK + 1 - 1] /= a[FIT_RANK + 1 - 1][FIT_RANK + 1 - 1]; //回代过程
    for (i = FIT_RANK + 1 - 2; i >= 0; i--)
    {
        double sum = 0;
        for (j = i + 1; j < FIT_RANK + 1; j++)
            sum += a[i][j] * b[j];
        b[i] = (b[i] - sum) / a[i][i];
    }
    //高斯列主元消去法结束
    printf("P(x) = %f+%fx+%fx^2\n", b[0], b[1], b[2]);

    /*
    for(i = 0; i < FIT_N; i++){  //误差比较
        double temp = b[0] + b[1] * this->x[i] + b[2] * this->x[i] * this->x[i] + b[3] * this->x[i] * this->x[i] * this->x[i];
        printf("%f    %f    error: %f\n", this->y[i], temp, temp - this->y[i]);
    }
    */

    return b;
}